/**
 * @file NewProtocolCurtis.cpp
 * @author SoBigDog (623644973lubin@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-11-10
 * 
 * @copyright Copyright (c) 2020
 * 
 */
#include "NewProtocolCurtis.h"

int16_t CNewProtocolCurtis::_speedVal = 0;
int16_t CNewProtocolCurtis::_forkVel = 0;
int16_t CNewProtocolCurtis::_lower_flag = 0;
int16_t CNewProtocolCurtis::_lift_flag = 0;
uint32_t CNewProtocolCurtis::_curtis_steer_range;

CNewProtocolCurtis::CNewProtocolCurtis()
{
	_encoderLine = 1024;
}

bool CNewProtocolCurtis::encode(motorDriverCmdType ct, int32_t val)
{
//	CanTxMsg msg;
//	msg.StdId = 0x203;
//	msg.IDE = CAN_Id_Standard;
//	msg.RTR = CAN_RTR_Data;
//	msg.DLC = 8;
//
//	if (DRV_CMD_TARGET_SPEED == ct) {
//		_speedVal = (int16_t)(val * 60.0f / (4.0f * _encoderLine));
//		return false;
//	} else if(DRV_CMD_TARGET_POS == ct) {
//		uint8_t driveRawDir;
//		uint16_t driveRawSpeed;
//		uint16_t steerRawAngle;
//
//		driveRawDir = (_speedVal < 0) ? 0x05: 0x03;
//		driveRawSpeed = (_speedVal < 0) ? -_speedVal : _speedVal;
//
//		steerRawAngle = val + _curtis_steer_range;
//	
//		msg.Data[0] = driveRawDir;
//		msg.Data[1] = driveRawSpeed & 0xFF;
//		msg.Data[2] = driveRawSpeed >> 8;
//		msg.Data[3] = 0x0a;			//acc
//		msg.Data[4] = 0x0a;			//acc
//		msg.Data[5] = (uint8_t)((steerRawAngle) & 0xFF);	//theta L
//		msg.Data[6] = (steerRawAngle) >> 8;					//theta H
//		msg.Data[7] = GlobalControl::getBatterySoc() * 100;
//		_canRouter->putMsg(msg);
//
//		msg.StdId = 0x303;
//		msg.Data[0] = ((uint8_t)_lift_flag << 1) | ((uint8_t)_lower_flag << 2);
//		if (_lift_flag == 1)
//		{
//			msg.Data[1] = _forkVel & 0xFF;
//			msg.Data[2] = _forkVel >> 8;		
//		} else {
//			msg.Data[1] = 0x00;
//			msg.Data[2] = 0x00;
//		}
//
//		if (_lower_flag == 1) {
//			msg.Data[3] = (uint16_t)(_forkVel * 32.727f) & 0xff;
//			msg.Data[4] = (uint16_t)(_forkVel * 32.727f) >> 8;
//		} else {
//			msg.Data[3] = 0x00;
//			msg.Data[4] = 0x00;
//		}
//		msg.Data[5] = 0x00;							//theta L
//		msg.Data[6] = 0x00;							//theta H
//		msg.Data[7] = 0x00;
//
//		_canRouter->putMsg(msg);
//
//	} else if (DRV_CMD_FORK_VEL == ct) {
//		_forkVel = val;
//		if (_forkVel > 0) {
//			_lift_flag = true;
//			_lower_flag = false;
//		} else if (_forkVel < 0) {
//			_lift_flag = false;
//			_lower_flag = true;
//			_forkVel = -_forkVel;
//		} else if (_forkVel == 0) {
//			_lift_flag = false;
//			_lower_flag = false;
//		}
//
//		return false;
//	}
	return true;
}
